using System;
using System.Collections;
using EarthUtility;
using Geometry;
using Unity.Mathematics;

namespace EarthGeospatial
{
    public class BoundingRegion
    {
        GlobeRectangle _rectangle;
        double _minimumHeight;
        double _maximumHeight;
        public OrientedBoundingBox _boundingBox;

        double3 _southwestCornerCartesian;
        double3 _northeastCornerCartesian;
        double3 _westNormal;
        double3 _eastNormal;
        double3 _southNormal;
        double3 _northNormal;
        bool _planesAreInvalid;

        public BoundingRegion(
             GlobeRectangle rectangle,
             double minimumHeight,
             double maximumHeight,
             Ellipsoid ellipsoid)
        {
            this._rectangle = rectangle;
            this._minimumHeight = minimumHeight;
            this._maximumHeight = maximumHeight;
            this._boundingBox = _computeBoundingBox(rectangle, minimumHeight, maximumHeight, ellipsoid);
        }
        public BoundingRegion(
       GlobeRectangle rectangle,
       double minimumHeight,
       double maximumHeight) : this(rectangle, minimumHeight, maximumHeight, Ellipsoid.WGS84)
        {

        }
        public static OrientedBoundingBox _computeBoundingBox(
           GlobeRectangle rectangle,
          double minimumHeight,
          double maximumHeight,
           Ellipsoid ellipsoid)
        {
            if (!Mathd.equalsEpsilon(
                    ellipsoid.getRadii().x,
                    ellipsoid.getRadii().y,
                    Mathd.EPSILON15))
            {
                throw new Exception("Ellipsoid must be an ellipsoid of revolution (radii.x == radii.y)");
            }
            double minX, maxX, minY, maxY, minZ, maxZ;
            Plane plane = new Plane(new double3(0.0, 0.0, 1.0), 0.0);
            if (rectangle.computeWidth() <= Mathd.ONE_PI)
            {
                Cartographic tangentPointCartographic = rectangle.computeCenter();
                double3 tangentPoint = ellipsoid.cartographicToCartesian(tangentPointCartographic);
                var tangentPlane = new EllipsoidTangentPlane(tangentPoint, ellipsoid);
                plane = tangentPlane.pPlane;
                double lonCenter = tangentPointCartographic.longitude;
                double latCenter = rectangle.getSouth() < 0.0 && rectangle.getNorth() > 0.0 ? 0.0 : tangentPointCartographic.latitude;

                // Compute XY extents using the rectangle at maximum height
                var perimeterCartographicNC = new Cartographic(
                   lonCenter,
                   rectangle.getNorth(),
                   maximumHeight);
                var perimeterCartographicNW = new Cartographic(
                    rectangle.getWest(),
                    rectangle.getNorth(),
                    maximumHeight);
                var perimeterCartographicCW = new Cartographic(
                   rectangle.getWest(),
                   latCenter,
                   maximumHeight);
                var perimeterCartographicSW = new Cartographic(
                    rectangle.getWest(),
                    rectangle.getSouth(),
                    maximumHeight);
                var perimeterCartographicSC = new Cartographic(
                   lonCenter,
                   rectangle.getSouth(),
                   maximumHeight);
                double3 perimeterCartesianNC =
   ellipsoid.cartographicToCartesian(perimeterCartographicNC);
                double3 perimeterCartesianNW =
                    ellipsoid.cartographicToCartesian(perimeterCartographicNW);
                double3 perimeterCartesianCW =
                   ellipsoid.cartographicToCartesian(perimeterCartographicCW);
                double3 perimeterCartesianSW =
                    ellipsoid.cartographicToCartesian(perimeterCartographicSW);
                double3 perimeterCartesianSC =
                   ellipsoid.cartographicToCartesian(perimeterCartographicSC);

                double2 perimeterProjectedNC =
                   tangentPlane.projectPointToNearestOnPlane(perimeterCartesianNC);
                double2 perimeterProjectedNW =
                   tangentPlane.projectPointToNearestOnPlane(perimeterCartesianNW);
                double2 perimeterProjectedCW =
                   tangentPlane.projectPointToNearestOnPlane(perimeterCartesianCW);
                double2 perimeterProjectedSW =
                   tangentPlane.projectPointToNearestOnPlane(perimeterCartesianSW);
                double2 perimeterProjectedSC =
                   tangentPlane.projectPointToNearestOnPlane(perimeterCartesianSC);

                minX = math.min(
                    math.min(perimeterProjectedNW.x, perimeterProjectedCW.x),
                    perimeterProjectedSW.x);

                maxX = -minX; // symmetrical

                maxY = math.max(perimeterProjectedNW.y, perimeterProjectedNC.y);
                minY = math.min(perimeterProjectedSW.y, perimeterProjectedSC.y);

                // Compute minimum Z using the rectangle at minimum height, since it will be
                // deeper than the maximum height
                perimeterCartographicNW.height = perimeterCartographicSW.height =
                    minimumHeight;
                perimeterCartesianNW =
                    ellipsoid.cartographicToCartesian(perimeterCartographicNW);
                perimeterCartesianSW =
                    ellipsoid.cartographicToCartesian(perimeterCartographicSW);

                minZ = math.min(
                    plane.getPointDistance(perimeterCartesianNW),
                    plane.getPointDistance(perimeterCartesianSW));
                maxZ = maximumHeight; // Since the tangent plane touches the surface at
                                      // height = 0, this is okay

                return fromPlaneExtents(
                    tangentPlane.Origin,
                    tangentPlane.XAxis,
                    tangentPlane.YAxis,
                    tangentPlane.ZAxis,
                    minX,
                    maxX,
                    minY,
                    maxY,
                    minZ,
                    maxZ);
            }

            bool fullyAboveEquator = rectangle.getSouth() > 0.0;
            bool fullyBelowEquator = rectangle.getNorth() < 0.0;
            double latitudeNearestToEquator =
               fullyAboveEquator ? rectangle.getSouth()
                                 : fullyBelowEquator ? rectangle.getNorth() : 0.0;
            double centerLongitude = rectangle.computeCenter().longitude;

            // Plane is located at the rectangle's center longitude and the rectangle's
            // latitude that is closest to the equator. It rotates around the Z axis. This
            // results in a better fit than the obb approach for smaller rectangles, which
            // orients with the rectangle's center normal.
            double3 planeOrigin = ellipsoid.cartographicToCartesian(new Cartographic(centerLongitude, latitudeNearestToEquator, maximumHeight));
            planeOrigin.z = 0.0; // center the plane on the equator to simpify plane
                                 // normal calculation
            bool isPole = math.abs(planeOrigin.x) < Mathd.EPSILON10 &&
                               math.abs(planeOrigin.y) < Mathd.EPSILON10;
            double3 planeNormal = !isPole ? math.normalize(planeOrigin) : new double3(1.0, 0.0, 0.0);
            var planeYAxis = new double3(0.0, 0.0, 1.0);
            double3 planeXAxis = math.cross(planeNormal, planeYAxis);
            plane = new Plane(planeOrigin, planeNormal);

            // Get the horizon point relative to the center. This will be the farthest
            // extent in the plane's X dimension.
            double3 horizonCartesian =
               ellipsoid.cartographicToCartesian(new Cartographic(
                   centerLongitude + Mathd.PI_OVER_TWO,
                   latitudeNearestToEquator,
                   maximumHeight));
            maxX = math.dot(plane.projectPointOntoPlane(horizonCartesian), planeXAxis);
            minX = -maxX; // symmetrical

            // Get the min and max Y, using the height that will give the largest extent
            maxY = ellipsoid
                       .cartographicToCartesian(new Cartographic(
                           0.0,
                           rectangle.getNorth(),
                           fullyBelowEquator ? minimumHeight : maximumHeight))
                       .z;
            minY = ellipsoid
                       .cartographicToCartesian(new Cartographic(
                           0.0,
                           rectangle.getSouth(),
                           fullyAboveEquator ? minimumHeight : maximumHeight))
                       .z;
            double3 farZ = ellipsoid.cartographicToCartesian(new Cartographic(
               rectangle.getEast(),
               latitudeNearestToEquator,
               maximumHeight));
            minZ = plane.getPointDistance(farZ);
            maxZ = 0.0; // plane origin starts at maxZ already

            // min and max are local to the plane axes
            return fromPlaneExtents(
                planeOrigin,
                planeXAxis,
                planeYAxis,
                planeNormal,
                minX,
                maxX,
                minY,
                maxY,
                minZ,
                maxZ);
        }
       public  CullingResult intersectPlane(Plane plane)
        {
            return this._boundingBox.intersectPlane(plane);
        }
        public double computeDistanceSquaredToPosition(double3 position)
        {
            return computeDistanceSquaredToPosition(position, Ellipsoid.WGS84);
        }
        public double computeDistanceSquaredToPosition(double3 position, Ellipsoid ellipsoid)
        {
            var cartographic = ellipsoid.cartesianToCartographic(position);
            if (!cartographic.HasValue)
            {
                return this._boundingBox.computeDistanceSquaredToPosition(position);
            }

            return this.computeDistanceSquaredToPosition(cartographic.Value, position);
        }
        public double computeDistanceSquaredToPosition(Cartographic position, Ellipsoid ellipsoid)
        {
            return computeDistanceSquaredToPosition(position, ellipsoid.cartographicToCartesian(position));
        }
        public double computeDistanceSquaredToPosition(Cartographic cartographicPosition, double3 cartesianPosition)
        {
            double result = 0.0;

            if (!this._planesAreInvalid)
            {
                if (!this._rectangle.contains(cartographicPosition))
                {
                    double3 southwestCornerCartesian = this._southwestCornerCartesian;
                    double3 northeastCornerCartesian = this._northeastCornerCartesian;
                    double3 westNormal = this._westNormal;
                    double3 southNormal = this._southNormal;
                    double3 eastNormal = this._eastNormal;
                    double3 northNormal = this._northNormal;

                    double3 vectorFromSouthwestCorner = cartesianPosition - southwestCornerCartesian;
                    double distanceToWestPlane = math.dot(vectorFromSouthwestCorner, westNormal);
                    double distanceToSouthPlane = math.dot(vectorFromSouthwestCorner, southNormal);

                    double3 vectorFromNortheastCorner = cartesianPosition - northeastCornerCartesian;
                    double distanceToEastPlane = math.dot(vectorFromNortheastCorner, eastNormal);
                    double distanceToNorthPlane = math.dot(vectorFromNortheastCorner, northNormal);

                    if (distanceToWestPlane > 0.0)
                    {
                        result += distanceToWestPlane * distanceToWestPlane;
                    }
                    else if (distanceToEastPlane > 0.0)
                    {
                        result += distanceToEastPlane * distanceToEastPlane;
                    }

                    if (distanceToSouthPlane > 0.0)
                    {
                        result += distanceToSouthPlane * distanceToSouthPlane;
                    }
                    else if (distanceToNorthPlane > 0.0)
                    {
                        result += distanceToNorthPlane * distanceToNorthPlane;
                    }
                }

                double cameraHeight = cartographicPosition.height;
                double minimumHeight = this._minimumHeight;
                double maximumHeight = this._maximumHeight;

                if (cameraHeight > maximumHeight)
                {
                    double distanceAboveTop = cameraHeight - maximumHeight;
                    result += distanceAboveTop * distanceAboveTop;
                }
                else if (cameraHeight < minimumHeight)
                {
                    double distanceBelowBottom = minimumHeight - cameraHeight;
                    result += distanceBelowBottom * distanceBelowBottom;
                }
            }

            double bboxDistanceSquared = this._boundingBox.computeDistanceSquaredToPosition(cartesianPosition);
            return math.max(bboxDistanceSquared, result);
        }

        private static OrientedBoundingBox fromPlaneExtents(double3 planeOrigin, double3 planeXAxis, double3 planeYAxis, double3 planeZAxis, double minimumX,
    double maximumX,
    double minimumY,
    double maximumY,
    double minimumZ,
    double maximumZ)
        {
            var halfAxes = new double3x3(planeXAxis, planeYAxis, planeZAxis);

            var centerOffset = new double3(
                (minimumX + maximumX) / 2.0,
                (minimumY + maximumY) / 2.0,
                (minimumZ + maximumZ) / 2.0);

            var scale = new double3(
                (maximumX - minimumX) / 2.0,
                (maximumY - minimumY) / 2.0,
                (maximumZ - minimumZ) / 2.0);

            var scaledHalfAxes = new double3x3(
               halfAxes[0] * scale.x,
               halfAxes[1] * scale.y,
               halfAxes[2] * scale.z);

            return new OrientedBoundingBox(planeOrigin + (math.mul(halfAxes, centerOffset)), scaledHalfAxes);
        }

        internal GlobeRectangle getRectangle()
        {
            return this._rectangle;
        }

        public BoundingRegion(GlobeRectangle globeRectangle)
        {
            this._rectangle = globeRectangle;
        }



        public static implicit operator bool(BoundingRegion foo)
        {
            return foo == null ? false : true;
        }

        internal double getMinimumHeight()
        {
            return this._minimumHeight;
        }

        internal double getMaximumHeight()
        {
            return this._maximumHeight;
        }

        internal BoundingRegion computeUnion(BoundingRegion other)
        {
            return new BoundingRegion(
        this._rectangle.computeUnion(other._rectangle),
      math.min(this._minimumHeight, other._minimumHeight),
      math.max(this._maximumHeight, other._maximumHeight));
        }
    }
}
